#ifndef _KF_ARM_IK_H
#define _KF_ARM_IK_H

#include "GSStm32F4_includes.h"
#define M_PI 3.14159f
#define M_PI_2 (_FLOAT)(M_PI/2.0f)
#define M_35_DEGREE 0.610865f


typedef struct{
    _FLOAT          d_link1[4][4];     //d_link1
    _FLOAT          d_link2[4][4];     //d_link1
    _FLOAT          d_link3[4][4];     //d_link1
    _FLOAT          d_link4[4][4];     //d_link1
    _FLOAT          d_link5[4][4];     //d_link1

    _FLOAT          d_link1Inv[4][4];     //d_link1
    _FLOAT          d_link2Inv[4][4];     //d_link1
    _FLOAT          d_link3Inv[4][4];     //d_link1
    _FLOAT          d_link4Inv[4][4];     //d_link1
    _FLOAT          d_link5Inv[4][4];     //d_link1
    _FLOAT          d_T[4][4];     //d_link1
    _FLOAT          d_T1[4][4];     //d_link1
    
    _FLOAT          d_linkTF[4][4];
    _FLOAT          d_angle[5];
    _FLOAT          d_abc[3];
    _FLOAT X[100];
}_ARM_IK_DATA;
extern _ARM_IK_DATA    g_ikData;
extern void g_FKCalMain(void);
extern void g_IKCalMain(void);
extern void s_getLinkTransform(_FLOAT * l_link, _FLOAT * l_output);

#endif /*_KF_ARM_IK_H*/
